#include "../include/client.h"
#include "../include/ui.h"
#include "../include/radardata.h"

Ui_MainWindow Gui;

#include <QApplication>
#include <QMainWindow>
#include <QTimer>
#include <ros/ros.h>
#include <QTextCodec>
#include <QObject>
#include <QPushButton>


geometry_msgs::PoseStamped relative_target_pose;
geometry_msgs::PoseStamped target_pose;




void GUIupdate(px4client *controler){
    relative_target_pose.pose.position.x = Gui.X_value->toPlainText().toDouble();
    relative_target_pose.pose.position.y = Gui.Y_value->toPlainText().toDouble();
    relative_target_pose.pose.position.z = Gui.Z_value->toPlainText().toDouble();


    Gui.relative_pos_value->setText("X: " + QString::number(relative_target_pose.pose.position.x, 'f', 2) + " "
                                 "Y: " + QString::number(relative_target_pose.pose.position.y, 'f', 2) + " "
                                 "Z: " + QString::number(relative_target_pose.pose.position.z, 'f', 2)
                                );
    if(controler->current_state.connected){
        //连接状态
        Gui.contact_state->setText("在线");
        Gui.contact_state->setStyleSheet(QString::fromUtf8("color: rgb(38, 162, 105);"));//(165, 29, 45)

        //无人机当前模式
        Gui.vehiclemode_state->setText(QString::fromStdString(controler->current_state.mode));
        Gui.vehiclemode_state->setStyleSheet(QString::fromUtf8("color: rgb(38, 162, 105);"));

        //解锁状态
        Gui.lock_state->setText(QString::fromStdString(controler->current_state.armed?"解锁":"上锁"));
        if (controler->current_state.armed)
        {
            Gui.lock_state->setStyleSheet(QString::fromUtf8("color: rgb(38, 162, 105);"));
        }else{
            Gui.lock_state->setStyleSheet(QString::fromUtf8("color: rgb(165, 29, 45);"));
        }

        //无人机当前位置更新
        Gui.vehiclepos_value->setText("X: " + QString::number(controler->local_pos.pose.pose.position.x, 'f', 2) + " "
                                      "Y: " + QString::number(controler->local_pos.pose.pose.position.y, 'f', 2) + " "
                                      "Z: " + QString::number(controler->local_pos.pose.pose.position.z, 'f', 2));

        //目标位置更新
        target_pose.pose.position.x = relative_target_pose.pose.position.x;
        target_pose.pose.position.y = relative_target_pose.pose.position.y;
        target_pose.pose.position.z = relative_target_pose.pose.position.z;
        
        //预计目标位置显示
        Gui.real_pos_value->setText("X: " + QString::number(target_pose.pose.position.x, 'f', 2) + " "
                                    "Y: " + QString::number(target_pose.pose.position.y, 'f', 2) + " "
                                    "Z: " + QString::number(target_pose.pose.position.z, 'f', 2));

        //当前任务目标位置显示
        Gui.now_task_value->setText("X: " + QString::number(controler->service_task.pose.position.x, 'f', 2) + " "
                                    "Y: " + QString::number(controler->service_task.pose.position.y, 'f', 2) + " "
                                    "Z: " + QString::number(controler->service_task.pose.position.z, 'f', 2));
        //显示任务状态
        if (controler->task_running)
        {
            Gui.task_state->setText("正在执行");
            Gui.task_state->setStyleSheet(QString::fromUtf8("color: rgb(165, 29, 45);"));
        }else{
            Gui.task_state->setText("未执行");
            Gui.task_state->setStyleSheet(QString::fromUtf8("color: rgb(38, 162, 105);"));
        }

    } else{
        Gui.contact_state->setText("离线");
        Gui.contact_state->setStyleSheet(QString::fromUtf8("color: rgb(165, 29, 45);"));
    }

}

int main(int argc, char** argv) {
    // 初始化 ROS
    ros::init(argc, argv, "qt_ros_app");
    ros::NodeHandle nh;

    px4client controler(&nh);
    RadarData radar_data(&nh, &Gui);
    // 初始化 Qt 应用程序
    QApplication app(argc, argv);

    // 设置 Qt 的编码为 UTF-8
    QTextCodec::setCodecForLocale(QTextCodec::codecForName("UTF-8"));
    QFont font("WenQuanYi Zen Hei",5);
    QApplication::setFont(font);
    // 创建主窗口
    QMainWindow From;
    Gui.setupUi(&From);
    From.show();
    

    QObject::connect(Gui.offmode, &QPushButton::clicked, &controler, &px4client::Requesting_Offboard);//绑定起飞按钮
    QObject::connect(Gui.landmode, &QPushButton::clicked, &controler, &px4client::Requesting_Landing);//绑定降落按钮

    QObject::connect(Gui.createtask, &QPushButton::clicked, [&controler]() {
        controler.create_task(target_pose);
    });

    QObject::connect(Gui.sendtask, &QPushButton::clicked, &controler, &px4client::operate_task);

    // 创建一个定时器，用于定期调用 ros::spinOnce
    QTimer timer;
    QObject::connect(&timer, &QTimer::timeout, [&](){
        // 让 ROS 处理所有的回调
        if (ros::ok()) {
            GUIupdate(&controler);
            radar_data.radarDraw();
            ros::spinOnce();
            // ros::AsyncSpinner spinner(1); // 使用 1 个线程进行 spin
            // spinner.start();
        } else {
            app.quit();
        }
    });

    // 设置定时器的间隔，通常设置为 10ms 左右
    timer.start(50);

    // 启动 Qt 的主循环
    return app.exec();
}
